#ifndef MANIPULATOR_H
#define MANIPULATOR_H

#include <iostream>
#include <stdio.h>
#include <fstream>
#include <math.h>
#include "serialstd.hpp"
using namespace std;

class Manipulator
{
    private:
    // manipulator frame has origin at the shoulder, zeroed at the undeployed position
    float S; // angle of shoulder joint [rad] in manipulator frame
    float E; // angle of elbow joint [rad]
    float W; // angle of wrist joint [rad] - currently not implemented! Set to zero!
    float V; // position of vertical actuator [mm]
    float G; // gripper opening [rad]

    float X; // tip position in x-direction in man. frame [mm]
    float Y; // tip position in y-direction in man. frame [mm]
    float Z; // tip position in z-direction in man. frame [mm]
    float C; // tip angle relative to S = 0 angle [deg]

    void  writeMicro();
    char convertangle(float);

    public:

    Manipulator(float, float, float, float, float,float,float); // constructor

     //~Manipulator(); // destructor
    void setJoints(float,float,float,float,float);
    void setPose(float,float,float,float,float);
    void Set(float,float,float,float,  float, float, float, float, float);
    float *getJoints();
    float *getPose();
    float getX();
    float getY();
    float getZ();
    float getC();

    float getS();
    float getE();
    float getW();
    float getV();
    float getG();
    int checkLimits(float,float,float,float,float,float,float,float,float);

    float *fwdKinematics(float,float,float,float);
    float *invKinematics(float,float,float,float);
    //float *incrementalPose(float,float,float,float);

    float L1; // length of shoulder-elbow link [mm]
    float L2; // length of elbow-wrist link [mm]

};

#endif // MANIPULATOR_H_INCLUDED
